
// ===============================================================================================================
// -*- C++ -*-
//
// Quaternion.cpp - Quaternion math.
//
// Copyright (c) 2011 Guilherme R. Lampert
// guilherme.ronaldo.lampert@gmail.com
//
// This code is licenced under the MIT license.
//
// This software is provided "as is" without express or implied
// warranties. You may freely copy and compile this source into
// applications you distribute provided that the copyright text
// above is included in the resulting source code.
//
// ===============================================================================================================

#include <Quaternion.hpp>

// =========================================================
// Quaternion Class Implementation
// =========================================================

Quaternion::Quaternion(void) : x(0), y(0), z(0), w(0)
{
	// Init everything to zero
}

Quaternion::Quaternion(const Quaternion & quat) : x(quat.x), y(quat.y), z(quat.z), w(quat.w)
{
	// Construct from copy
}

Quaternion::Quaternion(float X, float Y, float Z, float W) : x(X), y(Y), z(Z), w(W)
{
	// Construct from user defined data
}

Quaternion & Quaternion::operator = (float Val)
{
	x = Val;
	y = Val;
	z = Val;
	w = Val;
	return (*this);
}

Quaternion & Quaternion::operator = (const Quaternion & q)
{
	x = q.x;
	y = q.y;
	z = q.z;
	w = q.w;
	return (*this);
}

Quaternion Quaternion::operator * (const Vec3 & v) const
{
	return (Quaternion((w * v.x) + (y * v.z) - (z * v.y),
					   (w * v.y) + (z * v.x) - (x * v.z),
					   (w * v.z) + (x * v.y) - (y * v.x),
					  -(x * v.x) - (y * v.y) - (z * v.z)));
}

Quaternion Quaternion::operator * (const Quaternion & q) const
{
	return (Quaternion((x * q.w) + (w * q.x) + (y * q.z) - (z * q.y),
					   (y * q.w) + (w * q.y) + (z * q.x) - (x * q.z),
					   (z * q.w) + (w * q.z) + (x * q.y) - (y * q.x),
					   (w * q.w) - (x * q.x) - (y * q.y) - (z * q.z)));
}

void Quaternion::ComputeW(void)
{
	const float t = 1.0f - (x * x) - (y * y) - (z * z);

	if (t < 0.0f)
	{
		w = 0.0f;
	}
	else
	{
		w = -Math::Sqrt(t);
	}
}

float Quaternion::Length(void) const
{
	return (Math::Sqrt((x * x) + (y * y) + (z * z) + (w * w)));
}

void Quaternion::Normalize(void)
{
	const float invLen = Math::InvSqrt((x * x) + (y * y) + (z * z) + (w * w));
	x *= invLen;
	y *= invLen;
	z *= invLen;
	w *= invLen;
}

void Quaternion::RotatePoint(const Vec3 & in, Vec3 & out) const
{
	Quaternion inv(-x, -y, -z, w);
	inv.Normalize();

	// Multiply by vector
	Quaternion tmp((*this) * in);

	// Multiply by quaternion
	Quaternion final(tmp * inv);

	out.x = final.x;
	out.y = final.y;
	out.z = final.z;
}

float Quaternion::DotProduct(const Quaternion & quat) const
{
	return ((x * quat.x) + (y * quat.y) + (z * quat.z) + (w * quat.w));
}

Quaternion Quaternion::Slerp(const Quaternion & quat, float t) const
{
	// Check for out-of range parameter and return edge points if so:
	if (t <= 0.0f)
	{
		return (*this);
	}
	if (t >= 1.0f)
	{
		return (quat);
	}

	// Compute "cosine of angle between quaternions" using dot product:
	float cosOmega = DotProduct(quat);

	// If negative dot, use -q1.
	// Two quaternions q and -q represent the same rotation, but may produce
	// different slerp. We chose q or -q to rotate using the acute angle.
	float q1x = quat.x;
	float q1y = quat.y;
	float q1z = quat.z;
	float q1w = quat.w;

	if (cosOmega < 0.0f)
	{
		q1w = -q1w;
		q1x = -q1x;
		q1y = -q1y;
		q1z = -q1z;
		cosOmega = -cosOmega;
	}

	// We should have two unit quaternions, so dot should be <= 1.0
	// assert(cosOmega < 1.1f);

	// Compute interpolation fraction, checking for quaternions almost exactly the same:
	float k0, k1;

	if (cosOmega > 0.9999f)
	{
		// Very close - just use linear interpolation,
		// which will protect againt a divide by zero
		k0 = 1.0f - t;
		k1 = t;
	}
	else
	{
		// Compute the sin of the angle using the
		// trig identity sin^2(omega) + cos^2(omega) = 1
		const float sinOmega = Math::Sqrt(1.0f - (cosOmega * cosOmega));

		// Compute the angle from its sine and cosine:
		const float omega = Math::ArcTangent(sinOmega, cosOmega);

		// Compute inverse of denominator, so we only have to divide once:
		const float oneOverSinOmega = (1.0f / sinOmega);

		// Compute interpolation parameters:
		k0 = Math::Sine((1.0f - t) * omega) * oneOverSinOmega;
		k1 = Math::Sine(t * omega) * oneOverSinOmega;
	}

	// Interpolate and return new quaternion:

	return (Quaternion((k0 * x) + (k1 * q1x),
					   (k0 * y) + (k1 * q1y),
					   (k0 * z) + (k1 * q1z),
					   (k0 * w) + (k1 * q1w)));
}